#include "PSO.hpp"

void	PSO::resetD(unsigned inNVehicles)
{
	psoVehiclesMax = inNVehicles;
	psoGBest.veData.resize(inNVehicles);
	
	for (unsigned i = 0 ; i < psoParticles.size() ; i++)
	{
		psoParticles[i].resetD(inNVehicles);
		psoParticlesTmp[i].resetD(inNVehicles);
	}
}


void	PSO::execute()
{
	std::multimap<double, PSOParticle*>::iterator 	iv;
	double											fdr = -999999999;
	double											max_fdr = -99999999;
	double											up, ug, ul, un;
	unsigned										nvmin;
	bool                                            best_beaten = false;
    
    psoInitClock = clock();
    
	// 0. reset all
	std::cout << "PSO IT " << psoIt << std::endl;
	for (unsigned i = 0 ; i < psoParticles.size() ; i++)
		psoParticles[i].reset();
	
	
	
	psoEval.seVehicles = 0xFFFFFFFF;
	psoEval.seDistance = 99999999.;
	
	psoIt = 0;
	psoInertia = psoInertiaRange.first;
		
	for (;; psoIt++)
	{
        best_beaten = false;
		std::cout << "PSO IT " << psoIt << std::endl;
		
		// 1. decoder toutes les solutions
		for (unsigned i = 0 ; i < psoParticles.size() ; i++)
		{
			//std::cout << i << std::endl;
			//std::cout << psoParticles[i].toString() << std::endl;
			psoParticles[i].decode();
			//std::cout << psoParticles[i].toFitnessString() << std::endl;
		}
		
		
		//getchar();
		
		// 2. calculer les voisines de chaque particule
		for (unsigned i = 0 ; i < psoParticles.size() ; i++)
		{
			psoParticlesByDistance[i].clear();
			for (unsigned j = 0 ; j < psoParticles.size() ; j++)
			{
				if (i == j)
					continue;
				
				psoParticlesByDistance[i].insert(std::pair<double, PSOParticle*>(
							PSOParticle::getDistanceBetween(psoParticles[i], psoParticles[j]),
							&psoParticles[j])
					);
			}
		}
		
		nvmin = psoVehiclesMax;
		
		// 3. mise a jour personnal best & global best & ctor FDR
		for (unsigned i = 0 ; i < psoParticles.size() ; i++)
		{
			// personnal best
			if (psoParticles[i].getFitness() < psoParticles[i].paPBest.veFitness)
			{
				psoParticles[i].paPBest.veData = psoParticles[i].paX;
				psoParticles[i].paPBest.veFitness = psoParticles[i].getFitness();
			}
			
			if (psoParticles[i].paSolEval.seComplete && psoParticles[i].getNVehicles() < nvmin)
				nvmin = psoParticles[i].getNVehicles();
			
			// global best
            //std::cout << psoParticles[i].paSolEval.toString() << " : ";
            //std::cout << psoParticles[i].getFitness() << std::endl;
			if ((psoParticles[i].getFitness() < psoGBest.veFitness))
			{	
                //std::cout << "it best : ";
				//std::cout << psoParticles[i].paSolEval.toString() << std::endl;
				if (psoParticles[i].paSolEval < psoEval && psoParticles[i].paSolEval.seComplete)
				{
                    best_beaten = true;
                    std::cout << "record beaten : ";
					psoEval = psoParticles[i].paSolEval;
					std::cout << psoEval.toString() << " " << (double)((clock() - psoInitClock) / CLOCKS_PER_SEC) << "s" << std::endl;
				}
				//getchar();
				psoGBest.veData = psoParticles[i].paX;
				psoGBest.veFitness = psoParticles[i].getFitness();
			}
		}
		
        /*if (best_beaten)
            PSOParticle::NRand = 1;
        else
            PSOParticle::NRand++;
        */
        /*
		if (nvmin < psoVehiclesMax)
		{
			this->resetD(nvmin * 2);
			continue;
		}
		*/
		
		
		// refaire avec des multimaps c'est ca qui merde, le max_fdr n'est pas synchornise
		// correctement
		
		// lorsque le M = nombre de vehicules diminiue, reinitialiser tout le swarm
		// en N + 2 * M => normalement ca tourne, et plus on descend, plus le truc ira vite
		
		// 4. putain 3M * N^2, mak nbest
		for (unsigned i = 0 ; i < psoParticles.size() ; i++)
		{
			for (unsigned j = 0 ; j < psoParticles[i].paPBest.veData.size() ; j++) 
			{
				
				
				fdr = -99999.;
				max_fdr = fdr;
				
				for (unsigned k = 0 ; k < psoParticles.size() ; k++)
				{
					if (i == k)
						continue;
					
					fdr = ((psoParticles[i].getFitness() - psoParticles[k].paPBest.veFitness) 
										/ fabs(psoParticles[i].paX[j] - psoParticles[k].paPBest[j]));
					if (fdr > max_fdr)
					{
						//std::cout << max_fdr << std::endl;
						max_fdr = fdr;
						psoParticleBFDR[psoParticles[i].paPBest.veData.size() * i + j] = k;
					}
				}
				
				// valide dans cette loop ? cho cho
				//psoParticles[i].paNBest[j] = psoParticleBFDR[psoParticles[i].paPBest.veData.size() * i + j];
			}

		}
		
		for (unsigned i = 0 ; i < psoParticles.size() ; i++)
		{
			for (unsigned j = 0 ; j < psoParticles[i].paPBest.veData.size() ; j++) 
			{
				psoParticles[i].paNBest[j] = psoParticleBFDR[psoParticles[i].paPBest.veData.size() * i + j];
			}
		}
		
		
		// 5. mise a jour local best = min pbest immediate neighborhood
		for (unsigned i = 0 ; i < psoParticles.size() ; i++)
		{
			iv = psoParticlesByDistance[i].begin();
			psoParticles[i].paLBest = iv->second->paPBest;
			++iv;
			
			// tonnerre de PBest !
			for (unsigned register v = 1 ; v < psoNeighborhoodSize ; v++, ++iv)
			{
				if (iv->second->paPBest.veFitness < psoParticles[i].paLBest.veFitness)
				{
					psoParticles[i].paLBest = iv->second->paPBest;
				}
			}
		}
		
		
		//std::cout << u << std::endl;
		
		up = random_real();
		ug = random_real();
		ul = random_real();
		un = random_real();
		
		// 6. calcul de la new velo & new pos
		for (unsigned i = 0 ; i < psoParticles.size() ; i++)
		{
			for (unsigned j = 0 ; j < psoParticles[i].paVelo.size() ; j++)
			{
				
				psoParticles[i].paVelo[j] = 
							psoInertia * psoParticles[i].paVelo[j]
						+	psoAccP * up * (psoParticles[i].paPBest[j] - psoParticles[i].paX[j])
						+	psoAccG * ug * (psoGBest[j] - psoParticles[i].paX[j])
						+	psoAccL * ul * (psoParticles[i].paLBest[j] - psoParticles[i].paX[j])
						+	psoAccN * un * (psoParticles[i].paNBest[j] - psoParticles[i].paX[j])
						;
				
				psoParticles[i].paVelo[j] = floor(psoParticles[i].paVelo[j] * 100.) / 100.;
				
				if (psoParticles[i].paVelo[j] > psoVeloRange.second)
				{
					psoParticles[i].paVelo[j] = psoVeloRange.second;
				}
				/*else if (psoParticles[i].paVelo[j] < psoVeloRange.first)
				{
					psoParticles[i].paVelo[j] = psoVeloRange.first;
				}*/
						
				psoParticles[i].paXPrev.veData[j] = psoParticles[i].paX[j];
				psoParticles[i].paX[j] += psoParticles[i].paVelo[j]; 
				
				psoParticles[i].paX[j] = floor(psoParticles[i].paX[j] * 100.) / 100.;
				
				if (psoParticles[i].paX[j] > psoPosRange.second)
				{
					psoParticles[i].paX[j] = psoPosRange.second;
				}
				else if (psoParticles[i].paX[j] < psoPosRange.first)
				{
					psoParticles[i].paX[j] = psoPosRange.first;
				}
			}
			
			psoParticles[i].paXPrev.veFitness = psoParticles[i].getFitness();
		}
		
		psoInertia = psoInertiaRange.second + (psoMaxIt - psoIt) * (psoInertiaRange.first - psoInertiaRange.second) / psoMaxIt;
 
	}
}
